#include <openvino/openvino.hpp>

#include <opencv2/dnn.hpp>
#include "opencv2/opencv.hpp"
#include "opencv2/core/core.hpp"
#include "opencv2/imgproc.hpp"
#include "opencv2/imgcodecs.hpp"

#include <sys/prctl.h>

#include <cstdlib>
#include <fstream>
#include <iostream>
#include <sstream>
#include <ctime>

#include <iterator>
#include <memory>
#include <sstream>
#include <string>
#include <vector>

#include <unistd.h>
#include <sys/time.h>

#include "yolo.h"
#include "threadsafe_queue.h"

#include "MulticastSender.h"
#include "MulticastReceiver.h"

#include "msgHandler/Handler.h"
#include "camera.h"

#include "spdlog/spdlog.h"
#include "spdlog/sinks/basic_file_sink.h"

#include "carInParkProc.h"
#include "udpTcp/UdpSocket.h"
#include "ParkDetProc.h"

using namespace std;
using namespace cv;

#define LOCATION_BAD_TIME_MS 20000
#define RECV_LOCATION_TIMEOUT_MS 1000
#define SEND_VERIFY_PORT 8016
#define SEND_VERIFY_IP ((char *)"127.0.0.1")

#define SENDLIST_LENGTH 60

Camera frontCam;
Camera backCam;

Yolo yolo;
// Yolo yoloParkFront;
// Yolo yoloParkUpper;

std::shared_ptr<spdlog::logger> kylog;

typedef struct
{
    float fx;
    float fy;
    float fHeading;
    char parkid[32];
    char roadName[64];
    char road_id[32];
}ParkingInfo;

typedef struct _STR_FUSION_LOCATION
{
    int cSystemStatus;
    int GPS_Status;
    float fXInMap;
    float fYInMap;
    float fHeadingInMap;
    double dLatitude;
    double dLongitude;
    unsigned long long ullTimestamp;
} STR_FUSION_LOCATION, *PSTR_FUSION_LOCATION;

typedef struct 
{
    int id;
    int carInParkRet;
    int locationStatus;
    unsigned long long timestamps;
    char frontImgPath[256];
    char backImgPath[256];
    YoloBox box;
    int score;//add a flag for zhixindu?
}ParkProceeImg;

typedef struct
{
    char parkid[32];
    char roadName[64];
    int carInParkRet;
    int carInParkRet1;
    YoloBox selectBox;
    YoloBox selectBox1;
    char frontImgPath[256];
    char frontImgPath1[256];//can be same with frontImgPath, no use
    char road_id[32];//路段编号
    int score;//add a flag for zhixindu?
    int locationStatus;
}SendVerifyStr;

typedef struct
{
    char parkid[32];
    char roadName[64];
    int hasCar;
    YoloBox carBox;
    YoloBox upCarBox;
    char midImg[256];
    char upmidImg[256];
    char road_id[32];//路段编号
    int score;//add a flag for pa2 zhixindu?
}SendVerifyXieFeiStr;  

typedef struct
{
    int status;//0: no use, 1: in use, 2:standby
    int adjestStatus;//-1： ignore, 0~2 status
    char roadName[64];
    char startParkid[32];
    char endParkid[32];
    char weightPathFront[256];
    char weightPathBack[256];
    char classesidTxtPath[256];
    float fXInMapStart;
    float fYInMapStart;
    float fHeadingInMapStart;
    float fXInMapEnd;
    float fYInMapEnd;
    float fHeadingInMapEnd;
    Yolo *pYoloFront;
    Yolo *pYoloBack;
    std::vector<int> parkClasses;
}ToGetParkid;

typedef struct 
{
    int ToGetParkidIndex;
    int indexFront;
    int indexBack;
    int carInParkRet;
    Rect retRect;
    int locationStatus;
}BackProcess;

typedef struct 
{
    int parkid;
    unsigned long long time;
}SendList;

int sendIndex = 0;
SendList sendList[SENDLIST_LENGTH];

std::vector<int> parkClasses;

threadsafe_queue<STR_FUSION_LOCATION> g_LocationQueue;
threadsafe_queue<BackProcess> g_BackProcessQueue;



std::vector<ParkingInfo> front_parkingVec;
std::vector<ToGetParkid> toGetParkids;

std::vector<ParkDetProc*> pParks;

volatile int g_cSystemStatus = 0;
volatile int g_GPS_Status = 0;

Handler parkDetHandler;

int aaa = 0;
int aaa1 = 1;

volatile unsigned long long lastLocationGood = 0;
volatile unsigned long long lastRecvLocation = 0;
volatile int locationStatus = 0;//0:normal, 1:2,5, 2:bad

bool getRoadNameByParkid(char *parkid, char *roadName, int len);
bool getRoadidByRoadName(char *roadName, char *roadid, int len);

//初始化日志全局变量
//@params:
//@return:
void initSpdlog(void)
{
    time_t timer = time(NULL);
    struct tm *tm = localtime(&timer);
    char buf[64];

    snprintf(buf, 64, "/home/ubuntu/test/log/%d-%d-%d_%d_%d_%d_parkdet.txt", tm->tm_year + 1900, tm->tm_mon + 1, tm->tm_mday,
             tm->tm_hour, tm->tm_min, tm->tm_sec);
    kylog = spdlog::basic_logger_mt("basic", buf, true);

    spdlog::set_pattern("[%Y-%m-%d %T.%e]:%v");
}

int atoilc(char *str, int len)
{
    int i = 0, ret = 0;
    char *p = str;
    
    for(; i < len; i++)
    {
        if(*p > '9' || *p < '0')
        {
            p++;
        }
    }

    ret = atoi(p);

    return ret;
}

bool isFileExist(char *path)
{
    bool bret = false;
    FILE *fp = fopen(path, "r");
 
    if(fp == NULL)
    {
        bret = false;
    }
    else 
    {
        fclose(fp);
        bret = true;
    }

    return bret;
}

void initToGetParkid(void)
{
    char buf[2048] = {0};

    FILE *fp = fopen("/home/ubuntu/data/station/station_pa2.txt", "r");
    if(fp == NULL)
    {
        kylog->error("read station_pa2.txt file error\n");
        return;
    }

    while(!feof(fp))
    {
        ParkDetProc *pPark;

        if(!fgets(buf, 1024, fp))//read one line
        {
            continue;
        }

        pPark = new ParkDetProc();

        sscanf(buf, "%s %s %s %s %s %s", pPark->roadName, pPark->startParkid, pPark->endParkid, 
                pPark->weightPathFront, pPark->weightPathBack, pPark->classesidTxtPath);

        if(!isFileExist(pPark->weightPathFront))
        {
            kylog->info("[] no exist!!!!", pPark->weightPathFront);
            sleep(5);
            exit(0);
        }
        if(!isFileExist(pPark->weightPathBack))
        {
            kylog->info("[] no exist!!!!", pPark->weightPathBack);
            sleep(5);
            exit(0);
        }
        if(!isFileExist(pPark->classesidTxtPath))
        {
            kylog->info("[] no exist!!!!", pPark->classesidTxtPath);
            sleep(5);
            exit(0);
        }

        pParks.push_back(pPark);
    }

    fclose(fp);
}

unsigned long long getTimeStamps(void)
{
    struct timeval tv;
    unsigned long long timeStamps = 0;

    gettimeofday(&tv, NULL);
    timeStamps = tv.tv_sec * 1000 + tv.tv_usec / 1000.0;

    return timeStamps;
}

void clearSendList(void)
{
    sendIndex = 0;
    for(int i=0;i<SENDLIST_LENGTH;i++)
    {
        sendList[i].parkid = 0;
    }
}

void recordSendList(int parkid)
{
    sendList[sendIndex].time = getTimeStamps();
    sendList[sendIndex].parkid = parkid;
    sendIndex++;
    if(sendIndex >= SENDLIST_LENGTH)
    {
        sendIndex = 0;
    }
}

bool hasSend(int parkid)
{
    bool bret = false;

    for(int i=0;i<SENDLIST_LENGTH;i++)
    {
        if(sendList[i].parkid == parkid && getTimeStamps() - sendList[i].time < 3 * 60 * 1000)
        {
            bret = true;
        }
        else if(sendList[i].parkid == 0)
        {
            break;
        }
    }

    return bret;
}

UdpSocket udpSocket;
void parkProceeCallback(void *arg, char *pData, int len)
{
    ParkProceeImg *pParkProceeImg = (ParkProceeImg *)pData;
    int count = 0;

    if(pData == nullptr)
    {
        return;
    }

    int id = pParkProceeImg->id;

    if(pParkProceeImg->locationStatus == 0)
    {
        kylog->info("id = {}, location ok, don't send", id);
    }
    else if(hasSend(id))
    {
        kylog->info("id = {}, park det already have!", id);
    }
    else
    {
        SendVerifyStr sendVerify;
        char *out;
        memset(&sendVerify, 0, sizeof(sendVerify));

        snprintf(sendVerify.parkid, sizeof(sendVerify.parkid), "%d", id);
        sendVerify.carInParkRet = pParkProceeImg->carInParkRet;
        sendVerify.carInParkRet1 = pParkProceeImg->carInParkRet;
        memcpy(&sendVerify.selectBox, &pParkProceeImg->box, sizeof(sendVerify.selectBox));
        memcpy(&sendVerify.selectBox1, &sendVerify.selectBox, sizeof(sendVerify.selectBox1));
        sendVerify.score = pParkProceeImg->score;
        sendVerify.locationStatus = pParkProceeImg->locationStatus;
        strncpy(sendVerify.frontImgPath, pParkProceeImg->frontImgPath, sizeof(sendVerify.frontImgPath));
        strncpy(sendVerify.frontImgPath1, pParkProceeImg->frontImgPath, sizeof(sendVerify.frontImgPath));

        getRoadNameByParkid(sendVerify.parkid, sendVerify.roadName, sizeof(sendVerify.roadName));
        getRoadidByRoadName(sendVerify.roadName, sendVerify.road_id, sizeof(sendVerify.road_id));

        recordSendList(id);

        out = (char *)&sendVerify;
        udpSocket.send(SEND_VERIFY_IP, SEND_VERIFY_PORT, out, sizeof(sendVerify));
        kylog->info("id = {}, send to verify!!! {}, {}", id, sendVerify.frontImgPath, sendVerify.frontImgPath1);
    }
}

bool getRoadNameByParkid(char *parkid, char *roadName, int len)
{
    bool bret = false;

    int parking_num = front_parkingVec.size();;
    for(int i=0;i<parking_num;i++)
    {
        if(strcmp(front_parkingVec[i].parkid, parkid) == 0)
        {
            strncpy(roadName, front_parkingVec[i].roadName, len);
            bret = true;
            break;
        }
    }

    return bret;
}

bool getRoadidByRoadName(char *roadName, char *roadid, int len)
{
    bool bret = false;

    int parking_num = front_parkingVec.size();;
    for(int i=0;i<parking_num;i++)
    {
        if(strcmp(front_parkingVec[i].roadName, roadName) == 0)
        {
            strncpy(roadid, front_parkingVec[i].road_id, len);
            bret = true;
            break;
        }
    }

    return bret;
}

//called after initToGetParkid()
void initFrontPark(void)
{
    bool bret = false;
    char buf[2048] = {0};

    FILE *fp_parking_front = fopen("/home/ubuntu/data/station/station_front.txt", "r");
    if(fp_parking_front == NULL)
    {
        kylog->error("read parking location configure file error\n");
        return;
    }

    while(!feof(fp_parking_front))
    {
        int id;
        float lat, lon; 
        int t0, t1, t2;
        char s0[32], s1[32];
        ParkingInfo info;
        memset(&info, 0, sizeof(info));

        if(!fgets(buf, 1024, fp_parking_front))//read one line
        {
            continue;
        }

        sscanf(buf, "%d %s %f %f %f %f %f %d %d %s %s %d", 
                &id, info.parkid, &info.fx, &info.fy, &lat, &lon, &info.fHeading, &t0, &t1, info.roadName, 
                info.road_id, &t2);

        front_parkingVec.push_back(info);
    }
    fclose(fp_parking_front);


    int length = pParks.size();
    for(int i=0;i<length;i++)
    {
        for(auto parking : front_parkingVec)
        {
            if(strncmp(parking.parkid, pParks[i]->startParkid, strlen(parking.parkid)) == 0)
            {
                kylog->info("startParkid = {}, fx = {}, fy = {}, head = {}", 
                        pParks[i]->startParkid, parking.fx, parking.fy, parking.fHeading);
                pParks[i]->fXInMapStart = parking.fx;
                pParks[i]->fYInMapStart = parking.fy;
            }
            else if(strncmp(parking.parkid, pParks[i]->endParkid, strlen(parking.parkid)) == 0)
            {
                kylog->info("endParkid = {}, fx = {}, fy = {}, head = {}", 
                        pParks[i]->endParkid, parking.fx, parking.fy, parking.fHeading);
                pParks[i]->fXInMapEnd = parking.fx;
                pParks[i]->fYInMapEnd = parking.fy;
            }
        }
    }
}

//接收定位平面坐标信息
//@params:
//@return: void
void *locationThread(void *)
{
    printf("locationThread is start\n");
    MulticastReceiver receiver((char *)"192.168.3.22", 10121);
    char recv_buf[1024] = {0};

    STR_FUSION_LOCATION strFusionLocation;
    unsigned long long ullCount = 0;

    kylog->info("front_parkingVec size = {}", front_parkingVec.size());

    while (1)
    {
        memset(recv_buf, 0, 1024);

        receiver.Recv(recv_buf, sizeof(recv_buf));

        memcpy((char *)&strFusionLocation, recv_buf, sizeof(STR_FUSION_LOCATION));

        STR_FUSION_LOCATION empty;
        while (g_LocationQueue.try_pop(empty))
        {
            usleep(10);
        }
        g_LocationQueue.push(strFusionLocation);

        lastRecvLocation = getTimeStamps();

        g_cSystemStatus = strFusionLocation.cSystemStatus;
        g_GPS_Status = strFusionLocation.GPS_Status;

        if(g_cSystemStatus == 2 && g_GPS_Status == 4)
        {
            lastLocationGood = getTimeStamps();
            locationStatus = 0;
        }
        else if(g_cSystemStatus == 2 && g_GPS_Status == 5)
        {
            if(getTimeStamps() - lastLocationGood > LOCATION_BAD_TIME_MS)
            {
                locationStatus = 1;
            }
        }
        else 
        {
            locationStatus = 2;
        }

        if (ullCount % (10) == 0)
        {
            // printf("fXInMap:%f fYInMap:%f ullTimestamp:%lld \n", strFusionLocation.fXInMap, strFusionLocation.fYInMap, strFusionLocation.ullTimestamp);

            //TODO:
            int length = pParks.size();
            for(int i=0;i<length;i++)
            {
                float dis = fabs(pParks[i]->fXInMapStart - strFusionLocation.fXInMap) 
                        + fabs(pParks[i]->fYInMapStart - strFusionLocation.fYInMap);

                float disEnd = fabs(pParks[i]->fXInMapEnd - strFusionLocation.fXInMap) 
                        + fabs(pParks[i]->fYInMapEnd - strFusionLocation.fYInMap);

                if(dis < 20 && pParks[i]->status == 0 && pParks[i]->adjestStatus == -1)
                {
                    pParks[i]->adjestStatus = 2;//to adjest to standby
                    kylog->info("start parkid = {}, should adjest to standby!", pParks[i]->startParkid);
                    
                }
            }
        }

        //TODO: check out of distance!!!!
        if (ullCount % (1000) == 0)
        {
            int length = pParks.size();
            for(int i=0;i<length;i++)
            {
                if(pParks[i]->status >= 1)
                {
                    float shortest = 999999;
                    int startid = atoilc(pParks[i]->startParkid, strlen(pParks[i]->startParkid));
                    int endid = atoilc(pParks[i]->endParkid, strlen(pParks[i]->endParkid));
                    
                    for(auto parking : front_parkingVec)
                    {
                        int parkingid = atoilc(parking.parkid, strlen(parking.parkid));
                        if(parkingid >= startid && parkingid <= endid)
                        {
                            float dis = fabs(parking.fx - strFusionLocation.fXInMap) 
                                    + fabs(parking.fy - strFusionLocation.fYInMap);
                            if(dis < shortest)
                            {
                                shortest = dis;
                            }
                        }
                        else if(parkingid <= startid && parkingid >= endid)
                        {
                            float dis = fabs(parking.fx - strFusionLocation.fXInMap) 
                                    + fabs(parking.fy - strFusionLocation.fYInMap);
                            if(dis < shortest)
                            {
                                shortest = dis;
                            }
                        }
                    }

                    if(shortest > 60)
                    {
                        kylog->info("startParkid = {} location is end!", pParks[i]->startParkid);
                        pParks[i]->adjestStatus = 0;
                    }
                }
            }
        }

        if(ullCount % (12000) == 0)
        {
            int length = pParks.size();
            for(int i=0;i<length;i++)
            {
                kylog->info("now status {}: status = {}, adjestStatus = {}", pParks[i]->startParkid, 
                        pParks[i]->status, pParks[i]->adjestStatus);
            }
        }

        ullCount++;
    }
}

bool checkParkProcStatus(std::vector<ParkDetProc*> &pParks)
{
    bool isStart = false;

    for(int i=0;i<pParks.size();i++)
    {
        if(pParks[i]->trylock())
        {
            pParks[i]->checkStatus();

            pParks[i]->unlock();
        }
        else 
        {
            kylog->info("{}, try lock failed!", pParks[i]->startParkid);
        }

        if(pParks[i]->status >= 1)
        {
            isStart = true;
        }
    }

    return isStart;
}

bool processParkDet(ParkDetProc *pPark, Mat &img, Mat &imgBack, int carInParkRet, int locationStatus, Rect &retRect)
{
    char save[256];
    bool bret = false;

    vector<int> compression_params;
    compression_params.push_back(IMWRITE_JPEG_QUALITY); //选择jpeg
    compression_params.push_back(30);                   //在这个填入你要的图片质量

    unsigned long long timestamps = getTimeStamps();
    
    if((carInParkRet & 0xf0) == 0x10)//position ok
    {
        bool isHas = false;
        int number0 = 0, number1 = 0;
        int frondid = pPark->YoloParkDetFront(img, number0);
        ParkProceeImg parkProceeImg;
        if (frondid != -1 && number0 >= 6)
        {
            int hret = parkDetHandler.removeMsg(frondid);
            parkProceeImg.carInParkRet = carInParkRet;
            parkProceeImg.box.x = retRect.x;
            parkProceeImg.box.y = retRect.y;
            parkProceeImg.box.w = retRect.width;
            parkProceeImg.box.h = retRect.height;
            parkProceeImg.timestamps = timestamps;
            parkProceeImg.id = frondid;
            parkProceeImg.locationStatus = locationStatus;
            parkProceeImg.score = number0;

            memset(parkProceeImg.frontImgPath, 0, sizeof(parkProceeImg.frontImgPath));
            memset(parkProceeImg.backImgPath, 0, sizeof(parkProceeImg.backImgPath));

            snprintf(parkProceeImg.frontImgPath, sizeof(parkProceeImg.frontImgPath), 
                    "/home/ubuntu/data/upload/%d_%llu_%d_only_parkDet_front.jpg",
                    frondid, timestamps, parkProceeImg.score);
            strcpy(parkProceeImg.backImgPath, parkProceeImg.frontImgPath);//backImgPath no use!

            cv::rectangle(img, retRect, cv::Scalar(0, 0, 255), 1, 1, 0);

            if(hret != -1)
            {
                cv::imwrite(parkProceeImg.frontImgPath, img, compression_params);
                parkDetHandler.sendMsgDelay(frondid, 500, &parkProceeImg, sizeof(parkProceeImg), 
                                NULL, parkProceeCallback);
            }
            else
            {
                cv::imwrite(parkProceeImg.frontImgPath, img, compression_params);
                parkDetHandler.sendMsgDelay(frondid, 1000, &parkProceeImg, sizeof(parkProceeImg), 
                                NULL, parkProceeCallback);
            }
            

            return true;
        }
        int backid = pPark->YoloParkDetBack(imgBack, number1);
        ParkProceeImg hisRet;
        kylog->info("carInPark is ok,frontid:{},backid:{}",frondid,backid);
    
        
        if(frondid == backid && frondid != -1)
        {
            if(number0 + number1 >= 10)
            {
                if(parkDetHandler.getMsgById(frondid, &hisRet, sizeof(hisRet)) > 0)
                {
                    isHas = true;
                    if(number0 + number1 < hisRet.score)
                    {
                        parkDetHandler.sendMsgDelay(frondid, 500, &hisRet, sizeof(hisRet), 
                                NULL, parkProceeCallback);
                        kylog->info("id = {}, history score is higher!", frondid);
                        return true;
                    }
                }

                parkDetHandler.removeMsg(frondid);

                parkProceeImg.carInParkRet = carInParkRet;
                parkProceeImg.box.x = retRect.x;
                parkProceeImg.box.y = retRect.y;
                parkProceeImg.box.w = retRect.width;
                parkProceeImg.box.h = retRect.height;
                parkProceeImg.timestamps = timestamps;
                parkProceeImg.id = frondid;
                parkProceeImg.locationStatus = locationStatus;
                parkProceeImg.score = number0 + number1;

                memset(parkProceeImg.frontImgPath, 0, sizeof(parkProceeImg.frontImgPath));
                memset(parkProceeImg.backImgPath, 0, sizeof(parkProceeImg.backImgPath));

                snprintf(parkProceeImg.frontImgPath, sizeof(parkProceeImg.frontImgPath), 
                        "/home/ubuntu/data/upload/%d_%llu_%d_parkDet_front.jpg",
                        frondid, timestamps, parkProceeImg.score);
                strcpy(parkProceeImg.backImgPath, parkProceeImg.frontImgPath);//backImgPath no use!

                cv::rectangle(img, retRect, cv::Scalar(0, 0, 255), 1, 1, 0);

                cv::imwrite(parkProceeImg.frontImgPath, img, compression_params);

                if(number0 + number1 >= 12)
                {
                    int debug = parkDetHandler.sendMsgDelay(frondid, 10, &parkProceeImg, 
                            sizeof(parkProceeImg), nullptr, parkProceeCallback);
                    kylog->info("id = {}, send directly!", frondid);
                    return true;
                }

                //delay send directly
                if(isHas)
                {
                    int debug = parkDetHandler.sendMsgDelay(frondid, 500, &parkProceeImg, 
                        sizeof(parkProceeImg), nullptr, parkProceeCallback);
                }
                else
                {
                    int debug = parkDetHandler.sendMsgDelay(frondid, 1000, &parkProceeImg, 
                        sizeof(parkProceeImg), nullptr, parkProceeCallback);
                }
                
            }
        }

        //adjest bret
        if(number0 > 5 || number1 > 5)
        {
            bret = true;
        }
    }

    return bret;
}

void *checkParkProcStatusThread(void *args)
{
    prctl(PR_SET_NAME,"checkParkProcStatusThread");
    sleep(5);

    kylog->info("checkParkProcStatusThread start!");

    while(true)
    {
        checkParkProcStatus(pParks);

        yolo.checkTimeout();

        sleep(1);
    }
}

void *BackupProcessThread(void *args)
{
    prctl(PR_SET_NAME,"BackupProcessThread");
    while(true)
    {
        bool isWeightChange = false;

        BackProcess backProcess = g_BackProcessQueue.wait_and_pop();
        int index = backProcess.ToGetParkidIndex;
        cv::Mat img, backImg;
        frontCam.getMatByIndex(img, backProcess.indexFront);
        backCam.getMatByIndex(backImg, backProcess.indexBack);
        
        pParks[index]->lock();

        if(pParks[index]->status > 0)
        {
            if(processParkDet(pParks[index], img, backImg, backProcess.carInParkRet, 
                    backProcess.locationStatus, backProcess.retRect))
            {
                pParks[index]->adjestStatus = 1;

                kylog->info("{} should change to main detect", pParks[index]->startParkid);

                for(int i=0;i<pParks.size();i++)
                {
                    if(i == index)
                    {
                        continue;
                    }
                    if(pParks[i]->status == 1)
                    {
                        kylog->info("startParkid = {} is end!", pParks[i]->startParkid);
                        pParks[i]->adjestStatus = 0;
                    }
                }
            }
        }

        pParks[index]->unlock();
    }
}

int main(void)
{
    int flag = 0;

    initSpdlog();

    auto kylog = spdlog::get("basic");
    kylog->info("version 20230404-V3.4");
    printf("version 20230404\r\n");
    spdlog::flush_every(std::chrono::seconds(3));

    vector<int> compression_params;
    compression_params.push_back(IMWRITE_JPEG_QUALITY); //选择jpeg
    compression_params.push_back(30);                   //在这个填入你要的图片质量

    initToGetParkid();
    initFrontPark();
    clearSendList();

    yolo.Init((char *)"/home/ubuntu/test/yolo/carInPark_512x288/weights/best_openvino_model/best.xml"); // carInPark

    frontCam.initShm(SHMNAME);
    backCam.initShm(SHMNAME2);

    parkDetHandler.startHandlerTask();

    pthread_t locationTask;
    int iRet = pthread_create(&locationTask, NULL, locationThread, NULL);
    if (0 != iRet)
    {
        printf("Create Location Receive Process thread failed.\n");
        return -1;
    }

    pthread_t BackupProcessTask1;
    iRet = pthread_create(&BackupProcessTask1, NULL, BackupProcessThread, NULL);
    if (0 != iRet)
    {
        printf("Create BackProcessTask1 thread failed.\n");
        return -1;
    }

    pthread_t BackupProcessTask2;
    iRet = pthread_create(&BackupProcessTask2, NULL, BackupProcessThread, NULL);
    if (0 != iRet)
    {
        printf("Create BackProcessTask1e thread failed.\n");
        return -1;
    }

    pthread_t checkParkProcStatusTask;
    iRet = pthread_create(&checkParkProcStatusTask, NULL, checkParkProcStatusThread, NULL);
    if (0 != iRet)
    {
        printf("Create checkParkProcStatusThread thread failed.\n");
        return -1;
    }

    int hisParkid = -1;

    int nowLocationStatus = 0;
    while(1)
    {
        flag = 0;
        for(int i=0;i<pParks.size();i++)
        {
            if(pParks[i]->status >= 1)
            {
                flag = 1;
            }
        }

        if(flag == 1)
        {
            int index = -1;
            cv::Mat img, imgBack;
            unsigned int indexBack = 0, indexFront = 0;
            char save[256];

            if(frontCam.getNewMat(img, indexFront))
            {
                backCam.getNewMat(imgBack, indexBack);

                std::vector<YoloBox> yoloBoxes;
                yoloBoxes = yolo.DetectWithTimeOutMs(img, 500);

                nowLocationStatus = locationStatus;
                if(getTimeStamps() - lastRecvLocation >= RECV_LOCATION_TIMEOUT_MS)
                {
                    kylog->info("recv location timeout!!!");
                    nowLocationStatus = 2;
                }
                
                unsigned long long timestamps = getTimeStamps();

                cv::Rect retRect;
                int mret = findCross(img, yoloBoxes, retRect);

                //TODO: get status = 1 to process, other send to backthread!
                for(int i=0;i<pParks.size();i++)
                {
                    if(pParks[i]->status == 1)
                    {
                        index = i;
                    }
                    else if(pParks[i]->status == 2)//pass to backup thread
                    {
                        BackProcess backProcess;
                        backProcess.ToGetParkidIndex = i;
                        backProcess.indexFront = indexFront;
                        backProcess.indexBack = indexBack;
                        backProcess.carInParkRet = mret;
                        memcpy(&backProcess.retRect, &retRect, sizeof(backProcess.retRect));
                        backProcess.locationStatus = nowLocationStatus;
                        g_BackProcessQueue.push(backProcess);
                    }
                }

                //no status = 1
                if(index == -1)
                {
                    usleep(10*1000);
                    continue;
                }

                pParks[index]->lock();
                if(pParks[index]->status == 1)
                {
                    processParkDet(pParks[index], img, imgBack, mret, nowLocationStatus, retRect);
                }
                pParks[index]->unlock();

                usleep(5 * 1000);
            }
            else
            {
                usleep(10*1000);
            }
        }
        else
        {
            usleep(500 * 1000);
        }
    }
}
